import launch
import launch_ros
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory , get_package_prefix
import os
from launch.actions import ExecuteProcess


def generate_launch_description():
    
    #获取功能包share路径
    urdf_package_path = get_package_share_directory('botarm_controller')
    default_xacro_path = os.path.join(urdf_package_path,'urdf','Sevenbot_urdf.xacro')
    # default_gazebo_worlf_path = os.path.join(urdf_package_path,'world','home.world')
    #声明一个urdf目录的参数，方便修改
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(default_xacro_path),description='加载的模型文件路径'
    )

    #通过文件路径，获取内容，并转换成参数数值对象，以供传入 robot_state_publisher
    sub_command_result = launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration('model')])
    robot_description_value = launch_ros.parameter_descriptions.ParameterValue(sub_command_result,value_type=str)

    return LaunchDescription([
        action_declare_arg_mode_path,

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'robot_description':robot_description_value}],),

        

        ExecuteProcess(
            cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
            output='screen'),

        # ExecuteProcess(
        #     cmd=['gzserver', '--verbose', '-s', 'libgazebo_ros_factory.so'],  # 使用gzserver而不是gazebo
        #     output='screen'),

        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            name='robot_spawner',
            output='screen',
            arguments=["-topic", "/robot_description", "-entity", "bazu"]),

    ])